/**
 * @file img_node.cpp
 * @brief 图像格式转换节点
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2020-12-29
 * 
 * @copyright Copyright (c) 2020  WHU-EIS
 * 
 */
#include <iostream>
#include <vector>
#include "ros/ros.h"
#include "image_transport/image_transport.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/opencv.hpp"
#include "opencv2/imgcodecs/legacy/constants_c.h"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/image_encodings.h"

using namespace cv;

// 接收缓存
std::vector<sensor_msgs::CompressedImage> imgBuffer;
/**
 * @brief 图像接收回调函数
 * @param  msg              接收到的图像消息
 */
void img_callback(const sensor_msgs::CompressedImage& msg)
{
    // 加入接收缓存
    imgBuffer.push_back(msg);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "img_node");
	ros::NodeHandle nh;

    ros::NodeHandle n("~");
    std::string sub_topic;
    n.param("image_topic", sub_topic, std::string("/camera/color/image_raw/compressed"));
    ROS_INFO("image topic: %s", sub_topic.c_str());

    ros::Subscriber img_sub = nh.subscribe(sub_topic.c_str(), 10, img_callback);
    // ros::Publisher img_pub = nh.advertise<sensor_msgs::Image>("img_node/img", 10);
    
    image_transport::ImageTransport it(nh);
    image_transport::Publisher img_pub = it.advertise("/camera/image", 1);
    // image_transport::Subscriber img_sub = it.subscribe("/usb_cam/image_raw/compressed", 10, img_callback);

    ros::Rate loop_rate(30);
    
    while(ros::ok())
    {
        if(imgBuffer.size() != 0)
        {
            sensor_msgs::CompressedImage compImg = imgBuffer[0];
            cv::Mat img = cv::imdecode(compImg.data, CV_LOAD_IMAGE_COLOR);
            
            sensor_msgs::ImagePtr cv_ptr = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
            
            img_pub.publish(cv_ptr);

            // 清空缓存
            imgBuffer.clear();    
        }
        ros::spinOnce();
        loop_rate.sleep();        
    }
    return 0;
}